clc;
clear;
close all;

L=0.1; 
m=0.5;
g=9.8;
gra_bool=1;
torque=0;
b=0.02;

h=0.00001;

q=0;
dq=0;
ddq=0;

time=[];
qlist=[];
dqlist=[];
ddqlist=[];

theta_expected=1/57.3;
theta_now=0;
sum_theta_err=0;
Kp=5;Ki=0.1;Kd=100;
latest_theta_err=0;
torque=0;
for t=0:h:1
    ddq=dynamic(L,m,g,torque,q,dq,b,gra_bool);
    dq=dq+ddq*h;
    q=q+dq*h;
    time=[time;t];
    qlist=[qlist;q];
    dqlist=[dqlist;dq];
    ddqlist=[ddqlist;ddq];
    if(rem(1000*t,1)==0)
        theta_now = q;
        theta_err = theta_expected - theta_now;
        sum_theta_err = sum_theta_err + theta_err;
        torque = Kp*theta_err + Ki*sum_theta_err + Kd*(theta_err - latest_theta_err);
        latest_theta_err = theta_err;
    end
end

figure(1)
plot(time,qlist)
title('位置轨迹')
grid on
figure(2)
plot(time,dqlist)
title('速度轨迹')
grid on
figure(3)
plot(time,ddqlist)
title('加速度轨迹')
grid on